#ifndef TRAPEZOID_H
#define TRAPEZOID_H

typedef struct {
    float a_accel;     // 加速度阶段的加速度
    float a_decel;     // 减速阶段的加速度
    float Ta;          // 加速时间
    float Tv;          // 匀速时间
    float Td;          // 减速时间
    float T;           // 总时间
    float v_c;         // 中间速度
    float x_acc_end;   // 加速结束时的位置
    float x_vel_end;   // 匀速结束时的位置
    float x0;          // 初始位置
    float x1;          // 目标位置
    float v0;          // 初始速度
    float v1;          // 结束速度
} Trapezoid;

#define FIFO_LENGTH 32
typedef struct {
    Trapezoid trap_fifo[FIFO_LENGTH];
    
    // 硬件互斥锁（RP2040提供32个硬件锁，这里使用两个）
    spin_lock_t* alloc_lock;   // 保护allocated的锁
    spin_lock_t* queue_lock;    // 保护队列操作的锁
    
    // 共享状态
    uint32_t allocated;        // 位掩码（32位足够FIFO_LENGTH=32）
    
    // 循环队列
    uint8_t queue_head;         // 队列头索引
    uint8_t queue_tail;         // 队列尾索引
    uint8_t queue_count;        // 队列元素数量
    Trapezoid* queue[FIFO_LENGTH]; // 指针队列
} Trapezoid_FIFO;

int trapezoid_calculate_motion(Trapezoid* trap, float x0, float x1, float v0, float v1, float vmax, float a);
float trapezoid_get_pos(Trapezoid* trap, float t);
float trapezoid_get_velocity(Trapezoid* trap, float t);

// 需要高效率，整个过程不要复制Trapezoid的元素
// 在RP2040 MCU上使用，会涉及多核心访问的问题，需要确保线程安全

// 初始化函数（需在程序启动时调用）
void trapezoid_fifo_init(Trapezoid_FIFO* fifo);
// 分配一个未用的Trapezoid，如果没有返回0
Trapezoid *trapezoid_alloc(Trapezoid_FIFO *fifo);
// 释放不再使用的Trapezoid
void trapezoid_free(Trapezoid_FIFO *fifo, Trapezoid *t);
// 向fifo中放入Trapezoid
int trapezoid_push(Trapezoid_FIFO *fifo, Trapezoid *t);
// 从fifo中弹出Trapezoid
Trapezoid *trapezoid_pop(Trapezoid_FIFO *fifo);

extern Trapezoid_FIFO trap_fifo_ctrl;
#endif